#!/usr/bin/env python3

import sys
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from general_interface.action import SwitchConnection


class ROSNode(Node):
    def __init__(self):
        super().__init__("connection_ui")
        self.action_client = ActionClient(self, SwitchConnection, "switch_connection")

    def send_goal(self, goal):
        goal_msg = SwitchConnection.Goal()
        goal_msg.command = goal
        goal_msg.timeout = 10.0
        if not self.action_client.wait_for_server(timeout_sec=0.1):
            self.get_logger().error("find connection action server failed")
            return None
        return self.action_client.send_goal_async(
            goal_msg, feedback_callback=self.on_feedback
        )

    def connect(self):
        self.future = self.send_goal(SwitchConnection.Goal.CONNECT)
        if self.future == None:
            self.get_logger().info("No server find")
            return
        self.future.add_done_callback(self.on_response)

    def disconnect(self):
        self.future = self.send_goal(SwitchConnection.Goal.DISCONNECT)
        if self.future == None:
            self.get_logger().info("No server find")
            return
        self.future.add_done_callback(self.on_response)

    def pause(self):
        self.future = self.send_goal(SwitchConnection.Goal.STOP)
        if self.future == None:
            self.get_logger().info("No server find")
            return
        self.future.add_done_callback(self.on_response)

    def on_response(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info("Goal rejected")
            return
        self.get_logger().info("Goal accepted")
        self.result_future = goal_handle.get_result_async()
        self.result_future.add_done_callback(self.on_result)

    def on_feedback(self, feedback:SwitchConnection.Feedback):
        current = feedback.current
        if current == SwitchConnection.Feedback.CONNECTING:
            self.get_logger().info("Feedback:Connecting...")
        elif current == SwitchConnection.Feedback.DISCONNECTING:
            self.get_logger().info("Feedback:Disconnecting...")
        elif current == SwitchConnection.Feedback.PAUSING:
            self.get_logger().info("Feedback:Pausing...")
        else:
            self.get_logger().info(f"Feedback:Unknown feedback,current={current}")

    def on_result(self, future):
        result = future.result().result
        if result.result == SwitchConnection.Result.PAUSED:
            self.get_logger().info("Result:Paused")
        elif result.result == SwitchConnection.Result.CONNECTED:
            self.get_logger().info("Result:Connected")
        elif result.result == SwitchConnection.Result.DISCONNECTED:
            self.get_logger().info("Result:Disconnected")
        else:
            self.get_logger().info(f"Result:Unknown result,code={result.result}")


def main(args=None):
    rclpy.init(args=args)
    node = ROSNode()
    while rclpy.ok():
        command = input("Enter command (connect/disconnect/pause): ")
        if command == "connect":
            node.connect()
        elif command == "disconnect":
            node.disconnect()
        elif command == "pause":
            node.pause()
        else:
            print("Invalid command")
        rclpy.spin_once(node, timeout_sec=0.1)
    rclpy.shutdown()


if __name__ == "__main__":
    main()
